﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;

using starLiGHT.Input;
using starLiGHT.Physic.TwoD;
using starLiGHT.Physic;
using starLiGHT.Camera.TwoD;

#endregion

#region LICENSE

/*
    * This file is part of starLiGHT.Collision.
    * Copyright (c) 2007-2010 Roland Rosenkranz http://www.starLiGHT-Engine.de

    * This software is provided 'as-is', without any express or implied
    * warranty.  In no event will the authors be held liable for any damages
    * arising from the use of this software.
    * Permission is granted to anyone to use this software for any purpose,
    * including commercial applications, and to alter it and redistribute it
    * freely, subject to the following restrictions:
    * 1. The origin of this software must not be misrepresented; you must not
    * claim that you wrote the original software. If you use this software
    * in a product, an acknowledgment in the product documentation would be
    * appreciated but is not required.
    * 2. Altered source versions must be plainly marked as such, and must not be
    * misrepresented as being the original software.
    * 3. This notice may not be removed or altered from any source distribution.
    
    * ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Original Box2D License

/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#endregion

namespace starLiGHT.Sandbox.Physic2D
{
    /// Test settings. Some can be controlled in the GUI.
    public class Settings
    {
        public Settings()
        {
            hz = 60.0f;
            velocityIterations = 8;
            positionIterations = 3;
            drawStats = 0;
            drawShapes = 1;
            drawJoints = 1;
            drawAABBs = 0;
            drawPairs = 0;
            drawContactPoints = 0;
            drawContactNormals = 0;
            drawContactForces = 0;
            drawFrictionForces = 0;
            drawCOMs = 0;
            drawProfile = 1;
            enableWarmStarting = 1;
            enableContinuous = 1;
            enableSubStepping = 0;
            pause = 0;
            singleStep = 0;
        }

        public float hz;
        public int velocityIterations;
        public int positionIterations;
        public int drawShapes;
        public int drawJoints;
        public int drawAABBs;
        public int drawPairs;
        public int drawContactPoints;
        public int drawContactNormals;
        public int drawContactForces;
        public int drawFrictionForces;
        public int drawCOMs;
        public int drawStats;
        public int drawProfile;
        public int enableWarmStarting;
        public int enableContinuous;
        public int enableSubStepping;
        public int pause;
        public int singleStep;
    };

    public struct ContactPoint
    {
        public Fixture fixtureA;
        public Fixture fixtureB;
        public Vector2 normal;
        public Vector2 position;
        public PointState state;
    };

    public class Test : ContactListener
    {
        private readonly static int k_maxContactPoints = 2048;

        protected Body m_groundBody;
	    protected AABB m_worldAABB;
	    protected ContactPoint[] m_points = new ContactPoint[k_maxContactPoints];
	    protected int m_pointCount;
	    protected DestructionListener m_destructionListener = new DestructionListener();
        protected DebugDraw m_debugDraw;
	    protected int m_textLine;
	    public World m_world;
	    internal Body m_bomb;
	    //TODO: protected MouseJoint m_mouseJoint;
	    protected Vector2 m_bombSpawnPoint;
	    protected bool m_bombSpawning;
	    protected Vector2 m_mouseWorld;
	    protected int m_stepCount;
        protected Profile m_maxProfile;
        protected float scale;
        protected Input.InputManager input;
        public Camera2D camera;

        const int m_NumGamepads = 4;
        protected int m_SelectedPlayerID = -1;
        readonly GamePadState[] m_Gamepads = new GamePadState[m_NumGamepads];

        protected bool HaveSelectedPlayer {
            get {
                return m_SelectedPlayerID != -1;
            }
        }

        protected GamePadState SelectedPlayer {
            get {
                return m_Gamepads[m_SelectedPlayerID];
            }
        }

        public Test(Microsoft.Xna.Framework.Game game, float scale)
        {
            this.scale = scale;
            Vector2 gravity = new Vector2(0.0f, -10.0f);
            Boolean doSleep = true;
            m_world = new World(gravity, doSleep);
            //if (PhysicSystem2D.Instance.World == null) 
                PhysicSystem2D.Instance.World = m_world;
            m_bomb = null;
            m_textLine = 30;
            //TODO: m_mouseJoint = null;
            m_pointCount = 0;

            //TODO: m_destructionListener.test = this;
            m_world.SetDestructionListener(m_destructionListener);
            m_world.SetContactListener(this);
            m_world.SetDebugDraw(m_debugDraw);

            m_bombSpawning = false;

            m_stepCount = 0;

            BodyDef bodyDef = new BodyDef();
            m_groundBody = m_world.CreateBody(bodyDef);

            // Initialize input manager
            input = (Input.InputManager)game.Services.GetService(typeof(Input.InputManager));

            //TODO: re-enable camera control
            //input.addAction("CAMERA_RESET", new ControlSchemeButton(false, Keys.R));
            //input.addAction("CAMERA_ZOOM_IN", new ControlSchemeButton(true, Keys.PageDown));
            //input.addAction("CAMERA_ZOOM_OUT", new ControlSchemeButton(true, Keys.PageUp));
            //input.addAction("CAMERA_LEFT", new ControlSchemeButton(true, Keys.Left));
            //input.addAction("CAMERA_RIGHT", new ControlSchemeButton(true, Keys.Right));
            //input.addAction("CAMERA_UP", new ControlSchemeButton(true, Keys.Up));
            //input.addAction("CAMERA_DOWN", new ControlSchemeButton(true, Keys.Down));
            //input.addAction("SPAWN_BOMB", new ControlSchemeButton(false, Keys.Space));
        }

        public void Initialize(Microsoft.Xna.Framework.Game game, GraphicsDevice graphicsDevice)
        {
            //camera = new starLiGHT.Render.Camera2D(game,
            //                                       null,
            //                                       ((m_worldAABB.upperBound + m_worldAABB.lowerBound) / 2) / this.scale,
            //                                       null,
            //                                       null,
            //                                       null,
            //                                       this.scale,
            //                                       null,
            //                                       this.scale * -2f,
            //                                       this.scale * 2f,
            //                                       null,
            //                                       null);

            camera = new Camera2D(m_worldAABB.lowerBound, new Vector2(graphicsDevice.Viewport.Width, graphicsDevice.Viewport.Height));
            camera.Zoom = this.scale;

#if !ZUNE
            m_debugDraw = new DebugRender(graphicsDevice, camera, m_worldAABB, graphicsDevice.Viewport.Width, graphicsDevice.Viewport.Height, scale);
#else
            m_debugDraw = new DebugRenderZune(graphicsDevice, camera, m_worldAABB, graphicsDevice.Viewport.Width, graphicsDevice.Viewport.Height, scale);
#endif
            m_world.SetDebugDraw(m_debugDraw);
        }

        public virtual void Draw(Settings settings, SpriteBatch spriteBatch, SpriteFont font)
        {
            m_textLine = 0;

            if (camera != null) m_debugDraw.updateCamera(camera, true);
            m_world.DrawDebugData();

            if (settings.pause != 0)
            {
                m_debugDraw.DrawString(spriteBatch, font, 5, m_textLine, "****PAUSED****");
                m_textLine += 15;
            }

            m_debugDraw.DrawString(spriteBatch, font, 5, m_textLine, "Test: " + this.GetType().Name);
            m_textLine += 15;

            if (settings.drawStats > 0)
            {
                //m_debugDraw.DrawString(spriteBatch, font, 5, m_textLine, String.Format("proxies(max) = {0}({1}), pairs(max) = {2}({3})", m_world.GetProxyCount(), 
                //                                                                                                      Settings.MaxProxies,
                //                                                                                                      m_world.GetPairCount(), 
                //                                                                                                      Settings.MaxPairs));
                m_textLine += 15;

                //m_debugDraw.DrawString(spriteBatch, font, 5, m_textLine, String.Format("Islands last frame / max = {0} / {1}", m_world.LastIslandCount, m_world.MaxIslandCount));
                //m_textLine += 15;

                //m_debugDraw.DrawString(spriteBatch, font, 5, m_textLine, String.Format("bodies/contacts/joints = {0}/{1}/{2}", m_world.GetBodyCount(), m_world.GetContactCount(), m_world.GetJointCount()));
                //m_textLine += 15;

                //TODO: m_debugDraw.DrawString(5, m_textLine, "heap bytes = {0}", _byteCount);
                //TODO: m_textLine += 15;
            }

            // Track maximum profile times
            {
                Profile p = m_world.GetProfile();
                m_maxProfile.step = System.Math.Max(m_maxProfile.step, p.step);
                m_maxProfile.collide = System.Math.Max(m_maxProfile.collide, p.collide);
                m_maxProfile.solve = System.Math.Max(m_maxProfile.solve, p.solve);
                m_maxProfile.solveTOI = System.Math.Max(m_maxProfile.solveTOI, p.solveTOI);
            }

            if (settings.drawProfile == 1)
            {
                Profile p = m_world.GetProfile();
                m_debugDraw.DrawString(spriteBatch, font, 5, m_textLine, String.Format("step (max) = {0} ({1})", p.step, m_maxProfile.step));
                m_textLine += 15;
                m_debugDraw.DrawString(spriteBatch, font, 5, m_textLine, String.Format("collide(max) = {0} ({1})", p.collide, m_maxProfile.collide));
                m_textLine += 15;
                m_debugDraw.DrawString(spriteBatch, font, 5, m_textLine, String.Format("solve (max) = {0} ({1})", p.solve, m_maxProfile.solve));
                m_textLine += 15;
                m_debugDraw.DrawString(spriteBatch, font, 5, m_textLine, String.Format("solveTOI (max) = {0} ({1})", p.solveTOI, m_maxProfile.solveTOI));
                m_textLine += 15;
            }

            //if (m_mouseJoint)
            //{
            //    Body* body = m_mouseJoint->GetBody2();
            //    b2Vec2 p1 = body->GetWorldPoint(m_mouseJoint->m_localAnchor);
            //    b2Vec2 p2 = m_mouseJoint->m_target;

            //    glPointSize(4.0f);
            //    glColor3f(0.0f, 1.0f, 0.0f);
            //    glBegin(GL_POINTS);
            //    glVertex2f(p1.x, p1.y);
            //    glVertex2f(p2.x, p2.y);
            //    glEnd();
            //    glPointSize(1.0f);

            //    glColor3f(0.8f, 0.8f, 0.8f);
            //    glBegin(GL_LINES);
            //    glVertex2f(p1.x, p1.y);
            //    glVertex2f(p2.x, p2.y);
            //    glEnd();
            //}

            if (m_bombSpawning)
            {
                //glPointSize(4.0f);
                //glColor3f(0.0f, 0.0f, 1.0f);
                //glBegin(GL_POINTS);
                //glColor3f(0.0f, 0.0f, 1.0f);
                //glVertex2f(m_bombSpawnPoint.x, m_bombSpawnPoint.y);
                //glEnd();

                //glColor3f(0.8f, 0.8f, 0.8f);
                //glBegin(GL_LINES);
                //glVertex2f(m_mouseWorld.x, m_mouseWorld.y);
                //glVertex2f(m_bombSpawnPoint.x, m_bombSpawnPoint.y);
                //glEnd();
            }

            if (settings.drawContactPoints > 0)
            {
                //const float k_impulseScale = 0.1f;
                float k_axisScale = 0.3f;

                for (int i = 0; i < m_pointCount; ++i)
                {
                    ContactPoint point = m_points[i];

                    if (point.state == PointState.AddState)
                    {
                        // Add
                        m_debugDraw.DrawPoint(point.position, 10.0f, new Color(0.3f, 0.95f, 0.3f));
                    }
                    else if (point.state == PointState.PersistState)
                    {
                        // Persist
                        m_debugDraw.DrawPoint(point.position, 5.0f, new Color(0.3f, 0.3f, 0.95f));
                    }

                    if (settings.drawContactNormals == 1)
                    {
                        Vector2 p1 = point.position;
                        Vector2 p2 = p1 + k_axisScale * point.normal;
                        m_debugDraw.DrawSegment(p1, p2, new Color(0.4f, 0.9f, 0.4f));
                    }
                    else if (settings.drawContactForces == 1)
                    {
                        //b2Vec2 p1 = point->position;
                        //b2Vec2 p2 = p1 + k_forceScale * point->normalForce * point->normal;
                        //DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
                    }

                    if (settings.drawFrictionForces == 1)
                    {
                        //b2Vec2 tangent = b2Cross(point->normal, 1.0f);
                        //b2Vec2 p1 = point->position;
                        //b2Vec2 p2 = p1 + k_forceScale * point->tangentForce * tangent;
                        //DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
                    }
                }
            }
        }

        ~Test()
        {
            m_world = null;
        }

	    public void SetTextLine(int line) 
        { 
            m_textLine = line; 
        }

        public void DrawTitle(SpriteBatch spriteBatch, SpriteFont font, int x, int y, String text)
        {
            m_debugDraw.DrawString(spriteBatch, font, x, y, text);
        }

	    public virtual void Step(GraphicsDevice graphicsDevice, Settings settings)
        {
            KeyboardDevice keyboard = input.GetInputDevice(LogicalPlayerIndex.One, starLiGHT.Input.DeviceType.KEYBOARD) as KeyboardDevice;

            Keyboard(keyboard, input);
            //if (camera != null) camera.Update(input);
            camera.Update(new GameTime(), new RectangleF(Vector2.Zero, new Vector2(graphicsDevice.Viewport.Width, graphicsDevice.Viewport.Height)));

            float timeStep = settings.hz > 0.0f ? 1.0f / settings.hz : 0.0f;

	        if (settings.pause != 0)
	        {
		        if (settings.singleStep != 0)
		        {
			        settings.singleStep = 0;
		        }
		        else
		        {
			        timeStep = 0.0f;
		        }
	        }

#if !ZUNE
            if (m_debugDraw != null)
            {
                if (settings.drawShapes != 0) m_debugDraw.Flags |= DebugDraw.DrawFlags.Shape; else m_debugDraw.Flags = m_debugDraw.Flags & ~DebugDraw.DrawFlags.Shape;
                if (settings.drawJoints != 0) m_debugDraw.Flags |= DebugDraw.DrawFlags.Joint; else m_debugDraw.Flags = m_debugDraw.Flags & ~DebugDraw.DrawFlags.Joint;
                if (settings.drawAABBs != 0) m_debugDraw.Flags |= DebugDraw.DrawFlags.Aabb; else m_debugDraw.Flags = m_debugDraw.Flags & ~DebugDraw.DrawFlags.Aabb;
                if (settings.drawPairs != 0) m_debugDraw.Flags |= DebugDraw.DrawFlags.Pair; else m_debugDraw.Flags = m_debugDraw.Flags & ~DebugDraw.DrawFlags.Pair;
                if (settings.drawCOMs != 0) m_debugDraw.Flags |= DebugDraw.DrawFlags.CenterOfMass; else m_debugDraw.Flags = m_debugDraw.Flags & ~DebugDraw.DrawFlags.CenterOfMass;
            }
#endif

	        m_world.SetWarmStarting(settings.enableWarmStarting > 0);
	        m_world.SetContinuousPhysics(settings.enableContinuous > 0);
            m_world.SetSubStepping(settings.enableSubStepping > 0);

	        m_pointCount = 0;

	        m_world.Step(timeStep, settings.velocityIterations, settings.positionIterations);

	        if (timeStep > 0.0f)
	        {
		        ++m_stepCount;
	        }

        }

	    public virtual void Keyboard(KeyboardDevice keyboard, Input.InputManager input) 
        { 
            UpdateSelectedPlayer();
            //B2_NOT_USED(key); 
        }

	    public void ShiftMouseDown(Vector2 p)
        {
        }

	    public virtual void MouseDown(Vector2 p)
        {
        }

	    public virtual void MouseUp(Vector2 p)
        {
        }

	    public void MouseMove(Vector2 p)
        {
        }

	    public void LaunchBomb()
        {
	        Vector2 p = new Vector2(RandomFloat(-15.0f, 15.0f), 30.0f);
	        Vector2 v = -5.0f * p;
	        LaunchBomb(p, v);
        }

	    public void LaunchBomb(Vector2 position, Vector2 velocity)
        {
            if (m_bomb != null)
            {
                m_world.DestroyBody(m_bomb);
                m_bomb = null;
            }

            BodyDef bd = new BodyDef();
            bd.type = BodyType.DYNAMIC;
            bd.position = position;
            bd.bullet = true;
            m_bomb = m_world.CreateBody(bd);
            m_bomb.LinearVelocity = velocity;

            CircleShape circle = new CircleShape(); 
            circle.m_radius = 0.3f; 
            
            FixtureDef fd = new FixtureDef(); 
            fd.shape = circle; 
            fd.density = 20.0f; 
            fd.restitution = 0.1f; 
            
            Vector2 minV = position - new Vector2(0.3f, 0.3f); 
            Vector2 maxV = position + new Vector2(0.3f, 0.3f); 
            
            AABB aabb = new AABB(); 
            aabb.lowerBound = minV; 
            aabb.upperBound = maxV; 
            
            m_bomb.CreateFixture(fd); 
        }
    	
	    public void SpawnBomb(Vector2 worldPt)
        {
            m_bombSpawnPoint = worldPt;
            m_bombSpawning = true;
        }

	    public void CompleteBombSpawn(Vector2 p)
        {
            if (m_bombSpawning == false)
            {
                return;
            }

            const float multiplier = 30.0f;
            Vector2 vel = m_bombSpawnPoint - p;
            vel *= multiplier;
            LaunchBomb(m_bombSpawnPoint, vel);
            m_bombSpawning = false;
        }

	    // Let derived tests know that a joint was destroyed.
	    public virtual void JointDestroyed(Joint joint) 
        { 
            //B2_NOT_USED(joint); 
        }
	    
        public virtual void BoundaryViolated(Body body) 
        { 
            //B2_NOT_USED(body); 
        }

	    // Callbacks for derived classes.
	    public override void BeginContact(Contact contact) 
        { 
            //B2_NOT_USED(contact); 
        }

	    public override void EndContact(Contact contact) 
        { 
            //B2_NOT_USED(contact); 
        }

	    public override void PreSolve(Contact contact, Manifold oldManifold)
        {
	        Manifold manifold = contact.GetManifold();

	        if (manifold.pointCount == 0)
	        {
		        return;
	        }

	        Fixture fixtureA = contact.GetFixtureA();
	        Fixture fixtureB = contact.GetFixtureB();

	        PointState[] state1 = new PointState[PhysicSettings.MaxManifoldPoints], state2 = new PointState[PhysicSettings.MaxManifoldPoints];
            starLiGHT.Physic.TwoD.Collision.GetPointStates(state1, state2, oldManifold, manifold);

	        WorldManifold worldManifold;
	        contact.GetWorldManifold(out worldManifold);

	        for (int i = 0; i < manifold.pointCount && m_pointCount < k_maxContactPoints; ++i)
	        {
		        ContactPoint cp = m_points[m_pointCount];
		        cp.fixtureA = fixtureA;
		        cp.fixtureB = fixtureB;
		        cp.position = worldManifold.points[i];
		        cp.normal = worldManifold.normal;
		        cp.state = state2[i];
                m_points[m_pointCount] = cp;
		        ++m_pointCount;
	        }

            contact.SetManifold(manifold);
        }

	    public override void PostSolve(Contact contact, ContactImpulse impulse)
	    {
		    //B2_NOT_USED(contact);
		    //B2_NOT_USED(impulse);
	    }

        Random rand = new Random();
        /// Random number in range [-1,1]
        public float RandomFloat()
        {
	        float r = (float)(rand.Next() & (starLiGHT.Physic.TwoD.Math.RAND_LIMIT));
            r /= starLiGHT.Physic.TwoD.Math.RAND_LIMIT;
	        r = 2.0f * r - 1.0f;
	        return r;
        }

        /// Random floating point number in range [lo, hi]
        public float RandomFloat(float lo, float hi)
        {
            float r = (float)(rand.Next() & (starLiGHT.Physic.TwoD.Math.RAND_LIMIT));
            r /= starLiGHT.Physic.TwoD.Math.RAND_LIMIT;
	        r = (hi - lo) * r + lo;
	        return r;
        }

        protected void UpdateSelectedPlayer() {
            for(int i = 0; i < m_NumGamepads; i++) {
                m_Gamepads[i] = GamePad.GetState((PlayerIndex)i, GamePadDeadZone.None);
                if(m_SelectedPlayerID == -1 && m_Gamepads[i].IsConnected) {
                    m_SelectedPlayerID = i;
                }
            }
        }
    }

}
